#include <stdint.h>
#include <stddef.h>
#include "stdio.h"
#include "stm32f4xx_ll_gpio.h"
#include "iot_i2c.h"
#include "i2c_test.h"
#include "iot_errno.h"
#include "los_task.h"
#include "los_compiler.h"
#include "cmsis_os2.h"
#include "ohos_init.h"
#include "ohos_types.h"

void i2c_entry()
{
    printf("----- i2c test -----\n");
    IoTI2cInit(I2C_MPU6050, 50000);

    uint8_t mpu6050_cmd[5][2] = {
        {PWR_MGMT_1, 0x00},
        {SMPLRT_DIV, 0x07},
        {CONFIG, 0x06},
        {GYRO_CONFIG, 0x18},
        {ACCEL_CONFIG, 0x01},
    };
    for(uint8_t i=0; i<5; i++)
    {
        if(IoTI2cWrite(I2C_MPU6050, MPU6050Address, mpu6050_cmd[i], 2)!=IOT_SUCCESS){
            printf("write mpu6050 cmd %d failed...\n",  i);
            return;
        }
        LL_uDelay(10*1000);
    }
    uint8_t accx_h, accx_l;
    uint8_t accx_h_reg = ACCEL_XOUT_H;
    uint8_t accx_l_reg = ACCEL_XOUT_L;

    if(IoTI2cWrite(I2C_MPU6050, MPU6050Address, &accx_h_reg, 1) != IOT_SUCCESS){
        printf("write mpu6050 accx_h_reg failed...\n");
        return;
    }
    if(IoTI2cRead(I2C_MPU6050, MPU6050Address, &accx_h, 1) != IOT_SUCCESS){
        printf("read mpu6050 accx_h_reg failed...\n");
        return;
    }
    printf("axxx_h is %d...\n", accx_h);
    if(IoTI2cWrite(I2C_MPU6050, MPU6050Address, &accx_l_reg, 1) != IOT_SUCCESS){
        printf("write mpu6050 accx_l_reg failed...\n");
        return;
    }
    if(IoTI2cRead(I2C_MPU6050, MPU6050Address, &accx_l, 1) != IOT_SUCCESS){
        printf("read mpu6050 accx_l_reg failed...\n");
        return;
    }

    printf("accx is %d\n", ((uint16_t)accx_h<<8)+accx_l);
}

void i2c_task()
{
    UINT32 uwRet = -1;
    UINT32 taskID = 0;
    TSK_INIT_PARAM_S stTask = {0};
    stTask.pfnTaskEntry = (TSK_ENTRY_FUNC)i2c_entry;
    stTask.uwStackSize = 0x1000;
    stTask.pcName = "i2c task";
    stTask.usTaskPrio = 10;
    uwRet = LOS_TaskCreate(&taskID, &stTask);
    if (uwRet != LOS_OK) {
        printf("Task fs create failed\n");
    }
}